/*
 * @Description: 显示模块具体实现，// TODO:其实这里放优化的两个函数不太合适
 * @Author: Sang Hao
 * @Date: 2021-09-15 17:06:25
 * @LastEditTime: 2021-11-30 22:11:06
 * @LastEditors: Sang Hao
 */

#include <pcl/io/pcd_io.h>
#include <pcl/common/transforms.h>
#include "glog/logging.h"

#include "lidar_slam/global_defination/global_defination.h"
#include "lidar_slam/mapping/viewer/viewer.hpp"
#include "lidar_slam/models/cloud_filter/voxel_filter.hpp"
#include "lidar_slam/models/cloud_filter/no_filter.hpp"
#include "lidar_slam/models/cloud_filter/multi_plane_clipper_filter.hpp"
#include "lidar_slam/tools/file_manager.hpp"

#include "lidar_slam/models/segmentation/ransac_cluster_segment.hpp"
#include "lidar_slam/models/segmentation/mesh_dbscan_segment.hpp"
#include "lidar_slam/sensor_data/cloud_rgb_data.hpp"

namespace lidar_slam {
/**
 * @description: Public构造，直接初始化
 * @param  {*}
 * @return {*}
 */
Viewer::Viewer() {
	InitWithConfig();
}
/**
 * @description: 初始化配置
 * @param  {*}
 * @return {*}
 */
bool Viewer::InitWithConfig() {
	std::string config_file_path = WORK_SPACE_PATH + "/config/viewer.yaml";
	YAML::Node config_node = YAML::LoadFile(config_file_path);

	std::cout << "------------显示模块初始化---------------" << std::endl;
	InitParam(config_node);
	InitDataPath(config_node);
	InitFilter("frame", frame_filter_ptr_, config_node);
	InitFilter("local_map", local_map_filter_ptr_, config_node);
	InitFilter("global_map", global_map_filter_ptr_, config_node);
	InitSegment("global_map", global_map_segment_ptr_, config_node);
	
	return true;
}

/**
 * @description: 初始化参数，这里只有local_map_num数量这一个参数
 * @param  {*}
 * @return {*}
 */
bool Viewer::InitParam(const YAML::Node& config_node) {
	local_frame_num_ = config_node["local_frame_num"].as<int>();
	return true;
}

/**
 * @description: 初始化数据路径
 * @param  {*}
 * @return {*}
 */
bool Viewer::InitDataPath(const YAML::Node& config_node) {
	std::string data_path = config_node["data_path"].as<std::string>();
	if (data_path == "./") {
		data_path = WORK_SPACE_PATH;
	}
	std::cout << "初始化化数据路径 data_path : " << WORK_SPACE_PATH << std::endl;
	/* 先创建一级的文件夹，创建成功或者本来就有，都返回真 */
	if (!FileManager::CreateDirectory(data_path + "/slam_data")) {
		return false;
	}
	
	key_frames_path_ = data_path + "/slam_data/key_frames";
	map_path_ = data_path  + "/slam_data/map";

	if (!FileManager::InitDirectory(map_path_, "点云地图文件")) {
		return false;
	}

	return true;
}

/**
 * @description: 初始化滤波，使用者+指针+配置节点
 * @param  {*}
 * @return {*}
 */
bool Viewer::InitFilter(std::string filter_user,
						std::shared_ptr<CloudFilterInterface>& filter_ptr,
						const YAML::Node& config_node) {
	if (config_node["need_multi_plane_clipper_filter"].as<bool>()) {
		need_multi_plane_clipper_filter_ = true;
        multi_plane_clipper_filter_ptr_ = std::make_shared<MultiPlaneClipperFilter>(
            config_node["multi_plane_clipper_filter"]);
        std::cout << "完成多平面裁剪器的初始化!" << std::endl;
	}
	
	std::string filter_method = config_node[filter_user + "_filter"].as<std::string>();
	std::cout << "显示模块" << filter_user << "选择的滤波方法为" << filter_method << std::endl;
	
	/* 这里的no_filter和原来不一样， */
	if (filter_method == "voxel_filter") {
		filter_ptr = std::make_shared<VoxelFilter>(config_node[filter_method][filter_user]);
	} else if (filter_method == "no_filter") {
		filter_ptr = std::make_shared<NoFilter>();
	} else {
		LOG(ERROR) << "没有为" << filter_user << " 找到与 " << filter_method << " 相对应的滤波方法！";
		return false;
	}
	return true;
}

bool Viewer::InitSegment(std::string segment_user,
						std::shared_ptr<CloudSegmentInterface>& segment_ptr,
						const YAML::Node& config_node) {
	std::string segment_method = config_node[segment_user + "_segment"].as<std::string>();
	std::cout << "显示模块" << segment_user << "选择的分割方法为" << segment_method << std::endl;
	
	/* 这里的no_filter和原来不一样， */
	if (segment_method == "ransac_cluster_segment") {
		segment_ptr = std::make_shared<RansacClusterSegment>(
			config_node[segment_method][segment_user]);
	} else if (segment_method == "mesh_dbscan_segment") {
		segment_ptr = std::make_shared<MeshDbscanSegment>(
			config_node[segment_method][segment_user]);
	} else if (segment_method == "no_segment") {
		// filter_ptr = std::make_shared<NoFilter>();
	} else {
		LOG(ERROR) << "没有为" << segment_user << " 找到与 " << segment_method << " 相对应的分割方法！";
		return false;
	}
	return true;
}

/**
 * @description: 如果输入参数optimized_key_frames有数据，即有优化帧了，则更新到私有变量
 * 					内部调用OptimizeKeyFrames()函数
 * @param  {optimized_key_frames}
 * @return {*}
 */
bool Viewer::UpdateWithOptimizedKeyFrames(std::deque<KeyFrame>& optimized_key_frames) {
	has_new_global_map_ = false;
	if (optimized_key_frames.size() > 0) {
		optimized_key_frames_ = optimized_key_frames;
		optimized_key_frames.clear();
		OptimizeKeyFrames();
		has_new_global_map_ = true;
	}

	return has_new_global_map_;
}

/**
 * @description: //TODO 优化所有关键帧，这一步不太理解
 * @param  {*}
 * @return {*}
 */
bool Viewer::OptimizeKeyFrames() {
	size_t optimized_index = 0;
	size_t all_index = 0;
	while (optimized_index < optimized_key_frames_.size() && all_index < all_key_frames_.size()) {
		if (optimized_key_frames_.at(optimized_index).index < all_key_frames_.at(all_index).index) {
			optimized_index ++;
		} else if (optimized_key_frames_.at(optimized_index).index < all_key_frames_.at(all_index).index) {
			all_index ++;
		} else { 
			/* 找到了优化帧对应的位置，执行优化。*/
			/*//TODO 这里的优化执行了几次*/
			pose_to_optimize_ = optimized_key_frames_.at(optimized_index).pose * 
									all_key_frames_.at(all_index).pose.inverse();
			all_key_frames_.at(all_index) = optimized_key_frames_.at(optimized_index);
			optimized_index ++;
			all_index ++;
		}
	}

	while (all_index < all_key_frames_.size()) {
		/*这一步执行优化，将all_key_frames_中的每一帧都拉了一下*/ 
		all_key_frames_.at(all_index).pose = pose_to_optimize_ * all_key_frames_.at(all_index).pose;
		all_index ++;
	}

	return true;
}
/**
 * @description: 获得global_map，内部调用JointCloudMap函数
 * @param  {*}
 * @return {*}
 */
bool Viewer::JointGlobalMap(CloudData::CLOUD_PTR& global_map_ptr) {
	JointCloudMap(optimized_key_frames_, global_map_ptr);
	/* 加入地图时就把voxel滤波做了，这样之后就不做滤波了 */
	global_map_filter_ptr_->Filter(global_map_ptr, global_map_ptr);
	
	return true;
}
/**
 * @description: 获得local_map，内部调用JointCloudMap函数
 * @param  {*}
 * @return {*}
 */
bool Viewer::JointLocalMap(CloudData::CLOUD_PTR& local_map_ptr) {
	size_t begin_index = 0;
	// 这行代码表示了local_key_frames的帧数量
	if (all_key_frames_.size() > (size_t)local_frame_num_) {
		begin_index = all_key_frames_.size() - (size_t)local_frame_num_;
	}
	std::deque<KeyFrame> local_key_frames;
	for (size_t i = begin_index; i < all_key_frames_.size(); ++i) {
		local_key_frames.push_back(all_key_frames_.at(i));
	}

	JointCloudMap(local_key_frames, local_map_ptr);
	local_map_filter_ptr_->Filter(local_map_ptr, local_map_ptr);
	return true;
}
/**
 * @description: 功能函数，加入点云地图，供生成local_map和global_map
 * @param  {key_frames: 关键帧}
 * @return {map_cloud_ptr: 关键帧中的所有点云整合到map_cloud_ptr}
 */
bool Viewer::JointCloudMap(const std::deque<KeyFrame>& key_frames,
							CloudData::CLOUD_PTR& map_cloud_ptr) {
	map_cloud_ptr.reset(new CloudData::CLOUD());
	
	CloudData::CLOUD_PTR cloud_ptr(new CloudData::CLOUD());
	std::string file_path = "";
	for (size_t i = 0; i < key_frames.size(); ++i) {
		file_path = key_frames_path_ + "/key_frame_" + std::to_string(key_frames.at(i).index) + ".pcd";
		pcl::io::loadPCDFile(file_path, *cloud_ptr);
		pcl::transformPointCloud(*cloud_ptr, *cloud_ptr, key_frames.at(i).pose);
		*map_cloud_ptr += *cloud_ptr;
	}

	return true;
}
/**
 * @description: 当有new_key_frames_时，解析到all_key_frames_中，并更新里程计 
 * @param  {*}
 * @return {*}
 */
bool Viewer::UpdateWithNewKeyFrame(std::deque<KeyFrame>& new_key_frames,
	PoseData transformed_data, CloudData cloud_data) {
	has_new_local_map_ = false;
	if (new_key_frames.size() > 0) {
		KeyFrame key_frame;
		for (size_t i = 0; i < new_key_frames.size(); ++i) {
			key_frame = new_key_frames.at(i);
			key_frame.pose = pose_to_optimize_ * key_frame.pose;
			all_key_frames_.push_back(key_frame);
		}
		new_key_frames.clear();
		has_new_local_map_ = true;
	}

	optimized_odom_ = transformed_data;
	optimized_odom_.pose = pose_to_optimize_ * optimized_odom_.pose;

	optimized_cloud_ = cloud_data;
	pcl::transformPointCloud(*cloud_data.cloud_ptr, *optimized_cloud_.cloud_ptr, optimized_odom_.pose);
	
	return true;
}

/**
 * @description: 保存地图的服务
 * @param  {*}
 * @return {*}
 */
bool Viewer::SaveMap() {
	if (optimized_key_frames_.size() == 0) {
		return false;
	}
	/* 新建一个地图再保存 */
	CloudData::CLOUD_PTR global_map_ptr(new CloudData::CLOUD());
	
	/* 用JointGlobalMap，这个函数里已经对global_map_ptr做了滤波
	且内部调用的是 JointCloudMap(optimized_key_frames_, global_map_ptr) ;*/
	
	JointGlobalMap(global_map_ptr);
	// JointCloudMap(optimized_key_frames_, global_map_ptr);
	if (need_multi_plane_clipper_filter_) {
		multi_plane_clipper_filter_ptr_->Filter(global_map_ptr, global_map_ptr);
	}
	std::string map_file_path = map_path_ + "/map.pcd";
	pcl::io::savePCDFileBinary(map_file_path, *global_map_ptr);

	LOG(INFO) << "地图保存完成，地址是：" << std::endl << map_file_path << std::endl << std::endl;
	return true;
}

bool Viewer::SegMap(const CloudData::CLOUD_PTR& input_cloud_ptr, 
					CloudRGBData::CLOUD_PTR& seged_cloud_ptr) {
	/* 只做global_map的分割，不做current_scan和local_map的分割了 */
	global_map_segment_ptr_->Segmentation(input_cloud_ptr, seged_cloud_ptr);
	return true;
}

/**
 * @description: 获得当前优化的里程计位姿
 * @param  {*}
 * @return {*}
 */
Eigen::Matrix4f& Viewer::GetCurrentPose() {
	return optimized_odom_.pose;
}

/**
 * @description: 获得当前的激光scan并做voxel滤波，其实并不是current_scan，而是按里程计转换过的optimized_cloud
 * @param  {*}
 * @return {*}
 */
CloudData::CLOUD_PTR& Viewer::GetCurrentScan() {
	frame_filter_ptr_->Filter(optimized_cloud_.cloud_ptr, optimized_cloud_.cloud_ptr);
	return optimized_cloud_.cloud_ptr;
}

/**
 * @description: 获得local_map，并做voxel滤波
 * @param  {*}
 * @return {*}
 */
bool Viewer::GetLocalMap(CloudData::CLOUD_PTR& local_map_ptr) {
	JointLocalMap(local_map_ptr);
	/* 因为在加入地图时就做过滤波了，这里就不要了 */
	// local_map_filter_ptr_->Filter(local_map_ptr, local_map_ptr);
	return true;
}

/**
 * @description: 获得global_map，并做voxel滤波
 * @param  {*}
 * @return {*}
 */
bool Viewer::GetGlobalMap(CloudData::CLOUD_PTR& global_map_ptr) {
	JointGlobalMap(global_map_ptr);
	
	/* 因为在加入地图时就做过滤波了，这里就不要了 */
	// global_map_filter_ptr_->Filter(global_map_ptr, global_map_ptr);
	return true;
}

/**
 * @description: 小功能，返回是否有local_map
 * @param  {*}
 * @return {*}
 */
bool Viewer::HasNewLocalMap() {
	return has_new_local_map_;
}

/**
 * @description: 小功能，返回是否有global_map
 * @param  {*}
 * @return {*}
 */
bool Viewer::HasNewGlobalMap() {
	return has_new_global_map_;
}

}